#include <Windows.h>
#include "KinectRender.gdi+.h"
#include "Utils.h"
#include <string>
using namespace std;

#define  LINEWIDTH          6.0f
#define  HEADDIAMETER       50.0f
#define  HANDFOOTDIAMETER   24.0f
#define  JOINTDIAMETER      14.0f

// class KinectGdiRender
static ULONG_PTR gGdiPlusToken = 0;

static int initGdiPlus()
{
    if(gGdiPlusToken != 0)
        return 0;
    Gdiplus::GdiplusStartupInput gdiPlusStartupInput;
    return Gdiplus::GdiplusStartup(&gGdiPlusToken, &gdiPlusStartupInput, NULL);
}

static void termGdiPlus()
{
    if(gGdiPlusToken != 0)
    {
        Gdiplus::GdiplusShutdown(gGdiPlusToken);
        gGdiPlusToken = 0;
    }
}

static void verifyBitmap(int srcWidth, int srcHeight, Gdiplus::PixelFormat format, Gdiplus::Bitmap *& dstBitmap)
{
    if(dstBitmap != NULL)
    {
        if((dstBitmap->GetWidth() != srcWidth) || (dstBitmap->GetHeight() != srcHeight))
        {
            delete dstBitmap;
            dstBitmap = NULL;
        }
    }
    if(dstBitmap == NULL)
    {
        dstBitmap = new Gdiplus::Bitmap(srcWidth, srcHeight, format);
    }
}

static Gdiplus::Status copyBitsToBitmap(const void *srcBits, int srcStride, Gdiplus::Bitmap *dstBitmap)
{
    const int width  = dstBitmap->GetWidth();
    const int height = dstBitmap->GetHeight();

    Gdiplus::Rect rect(0, 0, width, height);
    Gdiplus::BitmapData bitmapData;
    Gdiplus::Status status = dstBitmap->LockBits(&rect, Gdiplus::ImageLockMode::ImageLockModeWrite, dstBitmap->GetPixelFormat(), &bitmapData);
    if(status == Gdiplus::Ok)
    {
        if(bitmapData.Stride == srcStride)
        {
            memcpy(bitmapData.Scan0, srcBits, srcStride*height);
        }
        else
        {
            byte *src = (byte *)(srcBits);
            byte *dst = (byte *)(bitmapData.Scan0);
            int stride = (srcStride < bitmapData.Stride) ? srcStride : bitmapData.Stride;
            for(int h = 0; h < height; h ++)
            {
                memcpy(dst, src, stride);
                src += srcStride;
                dst += bitmapData.Stride;
            }
        }
        dstBitmap->UnlockBits(&bitmapData);
    }
    return status;
}

static void drawBitmapToBitmap(Gdiplus::Bitmap *dstBitmap, Gdiplus::Bitmap *srcBitmap)
{
    Gdiplus::Graphics graphics(dstBitmap);
    graphics.SetInterpolationMode(Gdiplus::InterpolationModeHighQuality);
    graphics.DrawImage(srcBitmap, 0, 0, dstBitmap->GetWidth(), dstBitmap->GetHeight());
}

static bool GetEncoderClsid(LPCWSTR mimeType, CLSID * pClsid)     
{   
    UINT num = 0;
    UINT size = 0;
    Gdiplus::ImageCodecInfo *pImageCodecInfo = NULL;   
    Gdiplus::GetImageEncodersSize(&num, &size);   
    if(size == 0)
    {
        return false;
    }
    pImageCodecInfo = (Gdiplus::ImageCodecInfo *)(malloc(size));   
    if(pImageCodecInfo == NULL)
    {
        return false;
    }
    bool bFound = false;
    Gdiplus::Status status = GetImageEncoders(num, size, pImageCodecInfo);
    if(status == Gdiplus::Ok)
    {
        for(UINT j = 0; j < num; j ++)   
        {   
            if(_wcsicmp(pImageCodecInfo[j].MimeType, mimeType) == 0)   
            {   
                *pClsid = pImageCodecInfo[j].Clsid;
                bFound = true;
                break ;
            }           
        }
    }
    free(pImageCodecInfo);   
    return bFound;
}

bool KinectGdiRender::saveImageToFile(Gdiplus::Image *image, const WCHAR *fileName)
{
    int quality = 100; // jpeg only

    wstring wstr = fileName;
    int pos = (int)wstr.rfind(L'.');
    wstring extName = (pos > 0) ? wstr.substr(pos+1) : L"png";
    wstring mimeType = L"image/png";
    if(_wcsicmp(extName.c_str(), L"png") == 0)
    {
        mimeType = L"image/png";
    }
    else if(_wcsicmp(extName.c_str(), L"bmp") == 0)
    {
        mimeType = L"image/bmp";
    }
    else if(_wcsicmp(extName.c_str(), L"jpg") == 0)
    {
        mimeType = L"image/jpeg";
    }
    else if(_wcsicmp(extName.c_str(), L"gif") == 0)
    {
        mimeType = L"image/gif";
    }

    CLSID encoderClsid = GUID_NULL;
    if(!GetEncoderClsid(mimeType.c_str(), &encoderClsid))
    {
        return false;
    }
    Gdiplus::EncoderParameters encoderParameters;
    encoderParameters.Count                         = 1;
    encoderParameters.Parameter[0].Guid             = Gdiplus::EncoderQuality;
    encoderParameters.Parameter[0].Type             = Gdiplus::EncoderParameterValueTypeLong;     
    encoderParameters.Parameter[0].NumberOfValues   = 1;     
    encoderParameters.Parameter[0].Value            = &quality;
    
    Gdiplus::Status status = image->Save(fileName, &encoderClsid, &encoderParameters);
    return (status == Gdiplus::Ok);
}

KinectGdiRender::KinectGdiRender()
{
    initGdiPlus();

    mColorBitmap    = NULL;
    mDepthBitmap    = NULL;
    mDepthFrame     = NULL;
    mSkeletonBitmap = NULL;

    memset(&mSkeletonFrame, 0, sizeof(mSkeletonFrame));

    mSkeletonBackColor.SetValue(Gdiplus::Color::MakeARGB(255, 255, 255, 255));
    mTrackedLineColor.SetValue(Gdiplus::Color::MakeARGB(255, 0, 255, 0));
    mInferredLineColor.SetValue(Gdiplus::Color::MakeARGB(255, 128, 128, 128));
    mTrackedEllipseColor.SetValue(Gdiplus::Color::MakeARGB(255, 0, 0, 255));
    mInferredEllipseColor.SetValue(Gdiplus::Color::MakeARGB(255, 128, 128, 128));

    setScaleRatio(1.0f);
    setLookAtAngle(45.0, 45.0);

    mDrawPen = new Gdiplus::Pen(Gdiplus::Color());
    mDrawPen->SetStartCap(Gdiplus::LineCap::LineCapRound);
    mDrawPen->SetEndCap(Gdiplus::LineCap::LineCapRound);

    mDrawBrush = new Gdiplus::SolidBrush(Gdiplus::Color());

    mTextFont = new Gdiplus::Font(L"Arial", 12.0f, Gdiplus::FontStyleBold, Gdiplus::UnitPixel);
    mTextBrush = new Gdiplus::SolidBrush(Gdiplus::Color(255, 0, 0, 0));
}

KinectGdiRender::~KinectGdiRender()
{
    if(mColorBitmap != NULL)
    {
        delete mColorBitmap;
        mColorBitmap = NULL;
    }
    if(mDepthBitmap != NULL)
    {
        delete mDepthBitmap;
        mDepthBitmap = NULL;
    }
    if(mDepthFrame != NULL)
    {
        delete mDepthFrame;
        mDepthFrame = NULL;
    }
    if(mSkeletonBitmap != NULL)
    {
        delete mSkeletonBitmap;
        mSkeletonBitmap = NULL;
    }
    if(mDrawPen != NULL)
    {
        delete mDrawPen;
        mDrawPen = NULL;
    }
    if(mDrawBrush != NULL)
    {
        delete mDrawBrush;
        mDrawBrush = NULL;
    }
    if(mTextFont != NULL)
    {
        delete mTextFont;
        mTextFont = NULL;
    }
    if(mTextBrush != NULL)
    {
        delete mTextBrush;
        mTextBrush = NULL;
    }
}

void KinectGdiRender::setScaleRatio(float ratio)
{
    mLineWidth          = ratio*LINEWIDTH;
    mHeadDiameter       = ratio*HEADDIAMETER;
    mHandFootDiameter   = ratio*HANDFOOTDIAMETER;
    mJointDiameter      = ratio*JOINTDIAMETER;

    mScaleRatio         = ratio;
}

void KinectGdiRender::setLookAtAngle(float xAngle, float yAngle)
{
    mLookAtXAngle = xAngle;
    mLookAtYAngle = yAngle;
}

Gdiplus::Bitmap * KinectGdiRender::getColorBitmap()
{
    return mColorBitmap;
}

Gdiplus::Bitmap * KinectGdiRender::getDepthBitmap()
{
    return mDepthBitmap;
}

Gdiplus::Bitmap * KinectGdiRender::getSkeletonBitmap()
{
    return mSkeletonBitmap;
}

const NUI_SKELETON_FRAME & KinectGdiRender::getSkeletonFrame()
{
    return mSkeletonFrame;
}

void KinectGdiRender::drawColorToBitmap(KinectLib *pKinect, int width, int height, const KinectImageFrame &frame, bool drawSkeleton)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mColorBitmap);
    if(mColorBitmap == NULL)
    {
        return ;
    }
    if((width == frame.width) && (height == frame.height))
    {
        copyBitsToBitmap(frame.bits, frame.stride, mColorBitmap);
    }
    else
    {
        Gdiplus::Bitmap frameBitmap(frame.width, frame.height, frame.stride, PixelFormat32bppRGB, frame.bits);
        drawBitmapToBitmap(mColorBitmap, &frameBitmap);
    }
    if(drawSkeleton)
    {
        Gdiplus::Graphics graphics(mColorBitmap);
        drawSkeletonToGraphics(graphics, pKinect, width, height, mSkeletonFrame);
    }
}

static void verifyValueRange(int &value, int minValue, int maxValue)
{
    if(value < minValue) { value = minValue; }
    if(value > maxValue) { value = maxValue; }
}

static byte convertDepthToColor(int depth, int minDepth, int maxDepth)
{
//    verifyValueRange(depth, minDepth, maxDepth);
//    int color = ((depth-minDepth)*255)/(maxDepth-minDepth);

//    int color = ((depth >= minDepth) && (depth <= maxDepth)) ? (depth % 256) : 0;

    int realDepth = (depth >> 3);
    int color = (255 - ((255*realDepth)/0x1FFF));

    verifyValueRange(color, 0, 255);
    return byte(color);
}

void KinectGdiRender::drawDepthToBitmap(KinectLib *pKinect, int width, int height, const KinectDepthFrame &frame, bool drawSkeleton)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mDepthBitmap);
    verifyBitmap(frame.width, frame.height, PixelFormat32bppRGB, mDepthFrame);
    if((mDepthBitmap == NULL) || (mDepthFrame == NULL))
    {
        return ;
    }
    const bool isSameSize = ((width == frame.width) && (height == frame.height));

    Gdiplus::Bitmap *depthBitmap = isSameSize ? mDepthBitmap : mDepthFrame;

    Gdiplus::Rect rect(0, 0, frame.width, frame.height);
    Gdiplus::BitmapData bitmapData;
    Gdiplus::Status status = depthBitmap->LockBits(&rect, Gdiplus::ImageLockMode::ImageLockModeWrite, 
        PixelFormat32bppRGB, &bitmapData);
    if(status == Gdiplus::Ok)
    {
        byte *dst = (byte *)(bitmapData.Scan0);
        const NUI_DEPTH_IMAGE_PIXEL *depths = frame.depths;
        for(int h = 0; h < frame.height; h ++)
        {
            for(int w = 0; w < frame.width; w ++)
            {
                USHORT depth = depths->depth;
                byte color = convertDepthToColor(depth, frame.minDepth, frame.maxDepth);
                // BBGGRRAA
                dst[w*4+0] = color;
                dst[w*4+1] = color;
                dst[w*4+2] = color;
                dst[w*4+3] = 255;
                depths ++;
            }            
            dst += bitmapData.Stride;
        }
        depthBitmap->UnlockBits(&bitmapData);
    }
    if(!isSameSize)
    {
        drawBitmapToBitmap(mDepthBitmap, mDepthFrame);
    }
    if(drawSkeleton)
    {
        Gdiplus::Graphics graphics(mDepthBitmap);
        drawSkeletonToGraphics(graphics, pKinect, width, height, mSkeletonFrame);
    }
}

void KinectGdiRender::setSkeletonFrame(const NUI_SKELETON_FRAME &frame)
{
    mSkeletonFrame = frame;
}

void KinectGdiRender::drawBone(Gdiplus::Graphics &graphics, const NUI_SKELETON_DATA & skeletonData, 
                            const NUI_SKELETON_POSITION_INDEX joint0, const NUI_SKELETON_POSITION_INDEX joint1)
{
    NUI_SKELETON_POSITION_TRACKING_STATE joint0State = skeletonData.eSkeletonPositionTrackingState[joint0];
    NUI_SKELETON_POSITION_TRACKING_STATE joint1State = skeletonData.eSkeletonPositionTrackingState[joint1];
    if((joint0State == NUI_SKELETON_POSITION_NOT_TRACKED) || (joint1State == NUI_SKELETON_POSITION_NOT_TRACKED))
    {
        return ;
    }
    if((joint0State == NUI_SKELETON_POSITION_INFERRED) && (joint1State == NUI_SKELETON_POSITION_INFERRED))
    {
        return ;
    }
    if((joint0State == NUI_SKELETON_POSITION_TRACKED) && (joint1State == NUI_SKELETON_POSITION_TRACKED))
    {
        mDrawPen->SetColor(mTrackedLineColor);
        mDrawPen->SetWidth(mLineWidth);
    }
    else
    {
        mDrawPen->SetColor(mInferredLineColor);
        mDrawPen->SetWidth(mLineWidth);
    }
    graphics.DrawLine(mDrawPen, mSkeletonPoints[joint0], mSkeletonPoints[joint1]);
}

void KinectGdiRender::drawSkeletonToGraphics(Gdiplus::Graphics &graphics, KinectLib *pKinect, int width, int height, 
                                          const NUI_SKELETON_FRAME &frame)
{
    graphics.SetSmoothingMode(Gdiplus::SmoothingModeAntiAlias);

    for(int s = 0 ; s < NUI_SKELETON_COUNT; s ++)
    {
        const NUI_SKELETON_TRACKING_STATE trackingState = frame.SkeletonData[s].eTrackingState;
        const NUI_SKELETON_DATA & skeletonData = frame.SkeletonData[s];

        if(trackingState == NUI_SKELETON_TRACKED)
        {
            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                SkeletonPoint result = pKinect->convertSkeletonPosition(skeletonData.SkeletonPositions[p], width, height);
                mSkeletonPoints[p].X = result.x;
                mSkeletonPoints[p].Y = result.y;
            }

            // Render Torso
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_HEAD, NUI_SKELETON_POSITION_SHOULDER_CENTER);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_RIGHT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SPINE);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SPINE, NUI_SKELETON_POSITION_HIP_CENTER);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_RIGHT);

            // Left Arm
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SHOULDER_LEFT, NUI_SKELETON_POSITION_ELBOW_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_ELBOW_LEFT, NUI_SKELETON_POSITION_WRIST_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_WRIST_LEFT, NUI_SKELETON_POSITION_HAND_LEFT);

            // Right Arm
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_SHOULDER_RIGHT, NUI_SKELETON_POSITION_ELBOW_RIGHT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_ELBOW_RIGHT, NUI_SKELETON_POSITION_WRIST_RIGHT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_WRIST_RIGHT, NUI_SKELETON_POSITION_HAND_RIGHT);

            // Left Leg
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_HIP_LEFT, NUI_SKELETON_POSITION_KNEE_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_KNEE_LEFT, NUI_SKELETON_POSITION_ANKLE_LEFT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_ANKLE_LEFT, NUI_SKELETON_POSITION_FOOT_LEFT);

            // Right Leg
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_HIP_RIGHT, NUI_SKELETON_POSITION_KNEE_RIGHT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_KNEE_RIGHT, NUI_SKELETON_POSITION_ANKLE_RIGHT);
            drawBone(graphics, skeletonData, NUI_SKELETON_POSITION_ANKLE_RIGHT, NUI_SKELETON_POSITION_FOOT_RIGHT);

            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_TRACKED)
                {
                    mDrawBrush->SetColor(mTrackedEllipseColor);
                }
                else if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_INFERRED)
                {
                    mDrawBrush->SetColor(mInferredEllipseColor);
                }
                else
                {
                    continue ;
                }
                float ellipseDiameter = mJointDiameter;
                switch(p)
                {
                case NUI_SKELETON_POSITION_HEAD: 
                    {
                        ellipseDiameter = mHeadDiameter; 
                    }
                    break ;
                case NUI_SKELETON_POSITION_HAND_LEFT: 
                case NUI_SKELETON_POSITION_HAND_RIGHT: 
                case NUI_SKELETON_POSITION_FOOT_LEFT: 
                case NUI_SKELETON_POSITION_FOOT_RIGHT: 
                    {
                        ellipseDiameter = mHandFootDiameter; 
                    }
                    break ;
                }
                graphics.FillEllipse(mDrawBrush, mSkeletonPoints[p].X - (ellipseDiameter/2),
                    mSkeletonPoints[p].Y - (ellipseDiameter/2), ellipseDiameter, ellipseDiameter);
            }
        }
    }
}

void KinectGdiRender::drawSkeletonToBitmap(KinectLib *pKinect, int width, int height, const NUI_SKELETON_FRAME &frame)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mSkeletonBitmap);
    if(mSkeletonBitmap == NULL)
    {
        return ;
    }
    Gdiplus::Graphics graphics(mSkeletonBitmap);
    graphics.Clear(mSkeletonBackColor);
    drawSkeletonToGraphics(graphics, pKinect, width, height, frame);
}

void KinectGdiRender::draw3DBone(const NUI_SKELETON_DATA & skeletonData, 
                            const NUI_SKELETON_POSITION_INDEX joint0, const NUI_SKELETON_POSITION_INDEX joint1)
{
    NUI_SKELETON_POSITION_TRACKING_STATE joint0State = skeletonData.eSkeletonPositionTrackingState[joint0];
    NUI_SKELETON_POSITION_TRACKING_STATE joint1State = skeletonData.eSkeletonPositionTrackingState[joint1];
    if((joint0State == NUI_SKELETON_POSITION_NOT_TRACKED) || (joint1State == NUI_SKELETON_POSITION_NOT_TRACKED))
    {
        return ;
    }
    if((joint0State == NUI_SKELETON_POSITION_INFERRED) && (joint1State == NUI_SKELETON_POSITION_INFERRED))
    {
        return ;
    }
    if((joint0State == NUI_SKELETON_POSITION_TRACKED) && (joint1State == NUI_SKELETON_POSITION_TRACKED))
    {
        glColor3ub(mTrackedLineColor.GetRed(), mTrackedLineColor.GetGreen(), mTrackedLineColor.GetBlue());
        glLineWidth(mLineWidth);
    }
    else
    {
        glColor3ub(mInferredLineColor.GetRed(), mInferredLineColor.GetGreen(), mInferredLineColor.GetBlue());
        glLineWidth(mLineWidth);
    }

    glBegin(GL_LINES);
        glVertex3f(m3DPoints[joint0].x, m3DPoints[joint0].y, m3DPoints[joint0].z);
        glVertex3f(m3DPoints[joint1].x, m3DPoints[joint1].y, m3DPoints[joint1].z);
    glEnd();
}

static void convertSkeletonPoint3DPoint(int width, int height, const SkeletonPoint &point, Vector4 &result)
{
    width  /= 2;
    height /= 2;
    result.x =  ((float)point.x - (float)width)/(float)width;
    result.y =  (((float)point.y - (float)height)/(float)height);
    int z = point.depth;
    verifyValueRange(z, 400, 4000);
    int zdepth = (4000-400)/2;
    result.z = -(((float)(z-400) - (float)zdepth)/(float)zdepth);
}

#define PI  3.1416

static void drawSphere(GLfloat xx, GLfloat yy, GLfloat zz, GLfloat radius, GLfloat M, GLfloat N)
{
    float step_z = PI/M;
    float step_xy = 2*PI/N;
    float x[4],y[4],z[4];
    float angle_z = 0.0;
    float angle_xy = 0.0;
    int i=0, j=0;
    glBegin(GL_QUADS);
    for(i=0; i<M; i++)
    {
        angle_z = i * step_z;

        for(j=0; j<N; j++)
        {
            angle_xy = j * step_xy;
            x[0] = radius * sin(angle_z) * cos(angle_xy);
            y[0] = radius * sin(angle_z) * sin(angle_xy);
            z[0] = radius * cos(angle_z);
            x[1] = radius * sin(angle_z + step_z) * cos(angle_xy);
            y[1] = radius * sin(angle_z + step_z) * sin(angle_xy);
            z[1] = radius * cos(angle_z + step_z);
            x[2] = radius*sin(angle_z + step_z)*cos(angle_xy + step_xy);
            y[2] = radius*sin(angle_z + step_z)*sin(angle_xy + step_xy);
            z[2] = radius*cos(angle_z + step_z);
            x[3] = radius * sin(angle_z) * cos(angle_xy + step_xy);
            y[3] = radius * sin(angle_z) * sin(angle_xy + step_xy);
            z[3] = radius * cos(angle_z);
            for(int k=0; k<4; k++)
            {
                glVertex3f(xx+x[k], yy+y[k],zz+z[k]);
            }
        }
    }
    glEnd();
}

void KinectGdiRender::draw3DSkeletonToGraphics(Gdiplus::Graphics &graphics, KinectLib *pKinect, int width, int height, const NUI_SKELETON_FRAME &frame)
{
    if(!mMemoryOpenGL.initOpenGL(width, height))
    {
        return ;
    }

    glClearColor(1.0, 1.0, 1.0, 0.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    glLineWidth(mLineWidth);

    glRotatef(mLookAtXAngle, 1.0, 0.0, 0.0);
    glRotatef(mLookAtYAngle, 0.0, 1.0, 0.0);
    glRotatef(0.0, 0.0, 0.0, 1.0);

    for(int s = 0 ; s < NUI_SKELETON_COUNT; s ++)
    {
        const NUI_SKELETON_TRACKING_STATE trackingState = frame.SkeletonData[s].eTrackingState;
        const NUI_SKELETON_DATA & skeletonData = frame.SkeletonData[s];

        if(trackingState == NUI_SKELETON_TRACKED)
        {
            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                SkeletonPoint result = pKinect->convertSkeletonPosition(skeletonData.SkeletonPositions[p], width, height);
                convertSkeletonPoint3DPoint(width, height, result, m3DPoints[p]);
            }

            // Render Torso
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_HEAD, NUI_SKELETON_POSITION_SHOULDER_CENTER);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SHOULDER_RIGHT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_CENTER, NUI_SKELETON_POSITION_SPINE);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SPINE, NUI_SKELETON_POSITION_HIP_CENTER);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_HIP_CENTER, NUI_SKELETON_POSITION_HIP_RIGHT);

            // Left Arm
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_LEFT, NUI_SKELETON_POSITION_ELBOW_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_LEFT, NUI_SKELETON_POSITION_WRIST_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_WRIST_LEFT, NUI_SKELETON_POSITION_HAND_LEFT);

            // Right Arm
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_SHOULDER_RIGHT, NUI_SKELETON_POSITION_ELBOW_RIGHT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_ELBOW_RIGHT, NUI_SKELETON_POSITION_WRIST_RIGHT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_WRIST_RIGHT, NUI_SKELETON_POSITION_HAND_RIGHT);

            // Left Leg
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_HIP_LEFT, NUI_SKELETON_POSITION_KNEE_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_KNEE_LEFT, NUI_SKELETON_POSITION_ANKLE_LEFT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_LEFT, NUI_SKELETON_POSITION_FOOT_LEFT);

            // Right Leg
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_HIP_RIGHT, NUI_SKELETON_POSITION_KNEE_RIGHT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_KNEE_RIGHT, NUI_SKELETON_POSITION_ANKLE_RIGHT);
            draw3DBone(skeletonData, NUI_SKELETON_POSITION_ANKLE_RIGHT, NUI_SKELETON_POSITION_FOOT_RIGHT);

            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_TRACKED)
                {
                    glColor3ub(mTrackedEllipseColor.GetRed(), mTrackedEllipseColor.GetGreen(), mTrackedEllipseColor.GetBlue());
                }
                else if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_INFERRED)
                {
                    glColor3ub(mInferredEllipseColor.GetRed(), mInferredEllipseColor.GetGreen(), mInferredEllipseColor.GetBlue());
                }
                else
                {
                    continue ;
                }
                float ellipseDiameter = mJointDiameter;
                switch(p)
                {
                case NUI_SKELETON_POSITION_HEAD: 
                    {
                        ellipseDiameter = mHeadDiameter; 
                    }
                    break ;
                case NUI_SKELETON_POSITION_HAND_LEFT: 
                case NUI_SKELETON_POSITION_HAND_RIGHT: 
                case NUI_SKELETON_POSITION_FOOT_LEFT: 
                case NUI_SKELETON_POSITION_FOOT_RIGHT: 
                    {
                        ellipseDiameter = mHandFootDiameter; 
                    }
                    break ;
                }
//                drawSphere(m3DPoints[p].x, m3DPoints[p].y, m3DPoints[p].z, ellipseDiameter/2, 100.0, 100.0);
            }
        }
    }

    glFlush();

    Gdiplus::Bitmap frameBitmap(mMemoryOpenGL.getWidth(), mMemoryOpenGL.getHeight(), 
        mMemoryOpenGL.getWidth()*3, PixelFormat24bppRGB, (BYTE *)mMemoryOpenGL.getBits());
    graphics.SetInterpolationMode(Gdiplus::InterpolationModeHighQuality);
    graphics.DrawImage(&frameBitmap, 0, 0, frameBitmap.GetWidth(), frameBitmap.GetHeight());
}

void KinectGdiRender::draw3DSkeletonToBitmap(KinectLib *pKinect, int width, int height, const NUI_SKELETON_FRAME &frame)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mSkeletonBitmap);
    if(mSkeletonBitmap == NULL)
    {
        return ;
    }
    Gdiplus::Graphics graphics(mSkeletonBitmap);
    graphics.Clear(mSkeletonBackColor);
    draw3DSkeletonToGraphics(graphics, pKinect, width, height, frame);
}

void KinectGdiRender::drawPositionTextToGraphics(Gdiplus::Graphics &graphics, KinectLib *pKinect, int width, int height,
                                              const NUI_SKELETON_FRAME &frame)
{
    graphics.SetTextRenderingHint(Gdiplus::TextRenderingHintAntiAlias);

    for(int s = 0 ; s < NUI_SKELETON_COUNT; s ++)
    {
        const NUI_SKELETON_TRACKING_STATE trackingState = frame.SkeletonData[s].eTrackingState;
        const NUI_SKELETON_DATA & skeletonData = frame.SkeletonData[s];

        if(trackingState == NUI_SKELETON_TRACKED)
        {
            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                mSkeleton3Points[p] = pKinect->convertSkeletonPosition(skeletonData.SkeletonPositions[p], width, height);

                if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_TRACKED)
                {
                }
                else if(skeletonData.eSkeletonPositionTrackingState[p] == NUI_SKELETON_POSITION_INFERRED)
                {
                }
                else
                {
                    continue ;
                }
                
                Gdiplus::PointF textPoint((float)mSkeleton3Points[p].x, (float)mSkeleton3Points[p].y-12);

                wstring wstr = Utils::formatString(L"%d,%d,%d",
                    mSkeleton3Points[p].x, mSkeleton3Points[p].y, mSkeleton3Points[p].depth);

                graphics.DrawString(wstr.c_str(), wstr.length(), mTextFont, textPoint, mTextBrush);
            }
        }
    }
}

void KinectGdiRender::drawPositionTextToBitmap(KinectLib *pKinect, int width, int height, const NUI_SKELETON_FRAME &frame)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mSkeletonBitmap);
    if(mSkeletonBitmap == NULL)
    {
        return ;
    }
    Gdiplus::Graphics graphics(mSkeletonBitmap);
    drawPositionTextToGraphics(graphics, pKinect, width, height, frame);
}

void KinectGdiRender::drawAngle(Gdiplus::Graphics &graphics, const NUI_SKELETON_DATA & skeletonData, 
                             const NUI_SKELETON_POSITION_INDEX joint0, 
                             const NUI_SKELETON_POSITION_INDEX joint1, 
                             const NUI_SKELETON_POSITION_INDEX joint2)
{
    NUI_SKELETON_POSITION_TRACKING_STATE joint0State = skeletonData.eSkeletonPositionTrackingState[joint0];
    NUI_SKELETON_POSITION_TRACKING_STATE joint1State = skeletonData.eSkeletonPositionTrackingState[joint1];
    NUI_SKELETON_POSITION_TRACKING_STATE joint2State = skeletonData.eSkeletonPositionTrackingState[joint2];
    if((joint0State == NUI_SKELETON_POSITION_NOT_TRACKED) ||
        (joint1State == NUI_SKELETON_POSITION_NOT_TRACKED) ||
        (joint2State == NUI_SKELETON_POSITION_NOT_TRACKED))
    {
        return ;
    }
    double dAngle = Utils::calcAngle(
        mSkeleton3Points[joint0].x, mSkeleton3Points[joint0].y,
        mSkeleton3Points[joint1].x, mSkeleton3Points[joint1].y, 
        mSkeleton3Points[joint2].x, mSkeleton3Points[joint2].y,
        true);
    Gdiplus::PointF textPoint((float)mSkeleton3Points[joint1].x, (float)mSkeleton3Points[joint1].y+5);

    wstring wstr = Utils::formatString(L"%.3f", dAngle);

    graphics.DrawString(wstr.c_str(), wstr.length(), mTextFont, textPoint, mTextBrush);
}

void KinectGdiRender::drawAngleTextToGraphics(Gdiplus::Graphics &graphics, KinectLib *pKinect, int width, int height,
                                           const NUI_SKELETON_FRAME &frame)
{
    graphics.SetTextRenderingHint(Gdiplus::TextRenderingHintAntiAlias);

    for(int s = 0 ; s < NUI_SKELETON_COUNT; s ++)
    {
        const NUI_SKELETON_TRACKING_STATE trackingState = frame.SkeletonData[s].eTrackingState;
        const NUI_SKELETON_DATA & skeletonData = frame.SkeletonData[s];

        if(trackingState == NUI_SKELETON_TRACKED)
        {
            for(int p = 0; p < NUI_SKELETON_POSITION_COUNT; p ++)
            {
                mSkeleton3Points[p] = pKinect->convertSkeletonPosition(skeletonData.SkeletonPositions[p], width, height);
            }
            
            drawAngle(graphics, skeletonData, 
                NUI_SKELETON_POSITION_SHOULDER_CENTER, 
                NUI_SKELETON_POSITION_SHOULDER_LEFT,
                NUI_SKELETON_POSITION_ELBOW_LEFT);
            
            drawAngle(graphics, skeletonData, 
                NUI_SKELETON_POSITION_SHOULDER_CENTER, 
                NUI_SKELETON_POSITION_SHOULDER_RIGHT,
                NUI_SKELETON_POSITION_ELBOW_RIGHT);

            drawAngle(graphics, skeletonData, 
                NUI_SKELETON_POSITION_SHOULDER_LEFT, 
                NUI_SKELETON_POSITION_ELBOW_LEFT,
                NUI_SKELETON_POSITION_WRIST_LEFT);

            drawAngle(graphics, skeletonData, 
                NUI_SKELETON_POSITION_SHOULDER_RIGHT, 
                NUI_SKELETON_POSITION_ELBOW_RIGHT,
                NUI_SKELETON_POSITION_WRIST_RIGHT);
        }
    }
}

void KinectGdiRender::drawAngleTextToBitmap(KinectLib *pKinect, int width, int height, const NUI_SKELETON_FRAME &frame)
{
    verifyBitmap(width, height, PixelFormat32bppRGB, mSkeletonBitmap);
    if(mSkeletonBitmap == NULL)
    {
        return ;
    }
    Gdiplus::Graphics graphics(mSkeletonBitmap);
    drawAngleTextToGraphics(graphics, pKinect, width, height, frame);
}
